#include "bezier.h"


#define PI 3.14159

// For action ref
float action_ref_trot[16]= { -0.68873949,-2.7171507,    0.64782447,  -2.78440302,
    0.87747347,1.1122558,   -5.73509876,  -0.57981057,
    -2.78440302,-17.35424259,  -1.41528624,  -0.68873949,
    -0.57981057,2.25623534,   4.15258502,   0.87747347};
uint8_t action_ref_list[8] = {2,3,6,7,10,11,14,15};
uint8_t action_leg_indices[8] = {1,2,3,4,6,7,8,9};
float action_ref_final[16] ={0};
//float action[10] ={0.24504616, -0.11582746,  0.71558934, -0.46091432, -0.36284493,  0.00495828,
//    -0.06466855, -0.45247894,  0.72117291, -0.11068088};

//For action_to_xy
uint32_t xy_list[4] = {1,0,3,2};
float RT_OFFSET[2] = {0.23, 0.0};
// Multiplied thge scaling factor by 0.9 to be within the limits
float RT_SCALINGFACTOR[2] = {0.045*0.85/1.5, 0.85*(25./1.25)*(PI/180.0)};


void BezierUpdate(float *action) {

    /*
    Updates the reference trajectory using the action which is used in BezierInference
    :args action : Bezier coefficients generated by multiplying ARS matrix with the state
    Updates global variables
    */
    int i=0;

    for(i=0; i< 8; i ++) {
        action_ref_final[action_ref_list[i]] = action_ref_trot[action_ref_list[i]] + action[action_leg_indices[i]];
    }

    action_ref_final[0] = action_ref_final[11]; // r of left leg is matched with r of right leg
    action_ref_final[4] = action_ref_final[15]; // theta of left leg is matched with theta of right leg
    action_ref_final[8] = action_ref_final[3] ;// r of left leg is matched with r of right leg
    action_ref_final[12]= action_ref_final[7]; //theta of left leg is matched with theta of right leg

    action_ref_final[1] = 6 * action_ref_final[11] - action_ref_final[10]; // rdot of one leg is matched with rdot of opposite leg
    action_ref_final[5] = 6 * action_ref_final[15] - action_ref_final[14]; // thetadot of one leg is matched with thetadot of opposite leg
    action_ref_final[9] = 6 * action_ref_final[3] - action_ref_final[2]; // rdot of one leg is matched with rdot of opposite leg
    action_ref_final[13]= 6 * action_ref_final[7] - action_ref_final[6]; // thetadot of one leg is matched with thetadot of opposite leg

    return;
}


void BezierInference(float theta,float *xy) {
    /*
    This function calculates the xy point given the theta value for the Bezier curve. 
    :args theta : This is a single value that decides which values to update
    :args xy    : This is the variable in which the xy coordinates should be stored. Uses a few more global variable
    */    
    float phi[4]={0};
    float r_theta[4] = {0};
    float tau;
    int stance_leg = 0;
    int i=0, j=0;
    float final_action[16] ={0};
    if(theta < PI) 
    {
        tau = theta/PI;
        stance_leg = 0;
    }
    else 
    {
        tau = (theta - PI)/ PI;
        stance_leg = 1;
    }

    // Determine the Bezier polynomial
    if(stance_leg == 0)
    {
        for(i =0; i <16; i++)
        {
            final_action[i] = action_ref_final[i];
        }
    }
    else
    {
        for(i =0; i <16; i++)
        {
            if(i + 8 <16)
            {
                final_action[i] = action_ref_final[i+8];
            }
            else
            {
                final_action[i] = action_ref_final[i-8];
            }   
        }
    }
    
    phi[0] = (1-tau)*(1-tau)*(1-tau);
    phi[1] = tau*(1-tau)*(1-tau);
    phi[2] = tau*tau*(1-tau);
    phi[3] = tau*tau*tau;

    for(i=0; i<4; i++) {
        for(j=0; j<4; j++) {
            r_theta[i] += final_action[4*i + j]*phi[j];
        }
        // Scale the output
        r_theta[i] = r_theta[i]*RT_SCALINGFACTOR[i%2] + RT_OFFSET[i%2];
    }
    xy[0] = r_theta[0]*sinf(r_theta[1]);
    xy[1] = -r_theta[0]*cosf(r_theta[1]);
    xy[2] = r_theta[2]*sinf(r_theta[3]);
    xy[3] = -r_theta[2]*cosf(r_theta[3]);

}
